#include <REGX52.H>
#include "motor.h"
#include "delay.h"
#include "hc04.h"
#include "OLED.h"
#include "sg90.h"

/*#define BZ 1
#define XJ 2
#define GS 3*/

sbit A25 = P1^7;
sbit A26 = P3^0;
sbit A27 = P3^1;

sbit trace_left  = P2^7;
sbit trace_right = P2^6;

sbit follow_left  = P2^5;
sbit follow_right = P2^4;
sbit D5 = P3^7;//设备变量led1指向P3组IO口的第7口
sbit D6 = P3^6;//设备变量led2指向P3组IO口的第6口


extern  Trig;
extern	Echo;
	

int middle_dis;
int left_dis;
int right_dis;

void car_trace()
{
	if(trace_left == 0 && trace_right == 0){
		goForward();
	}
	 if(trace_left == 1 && trace_right== 0){
		 goLeft();
		 
	 }
	if(trace_left == 0 && trace_right == 1){
		goRight();
	 }
	if(trace_left == 1 && trace_right == 1){
		stop();
	 }
}

void car_follow()
{
	if(follow_left == 0 && follow_right == 1){
		goLeft();
	 }
	if(follow_left == 1 && follow_right == 0){
		goRight();
	 }
	if(follow_left == 0 && follow_right == 0){
		stop();
	 }
	if(follow_left == 1 && follow_right == 1){
		goForward();
	}
}

void car_elude()
{
	
	middle_dis = get_distance();
	if(middle_dis > 35){
		goForward();
	}
	else if(middle_dis < 10){
			goBack();
			Delay150ms();
			stop();
	}
	else{
			stop();
			sg_left();
			Delay450ms();
			left_dis = get_distance();
			
			sg_middle();
			Delay450ms();
			
			sg_right();
			Delay450ms();
			right_dis = get_distance();
			
			sg_middle();
			Delay450ms();
			
			if(left_dis < right_dis){
				goRight();
				Delay150ms();
				stop();
			}
			if(left_dis > right_dis){
				goLeft();
				Delay150ms();
				stop();
			}
		}	
}



void main()
{
	//int mark;
	Delay1000ms();
	InitTime0();
	Time1Init();

	sg_middle();
	Delay450ms();
	OLED_Init();
	OLED_Clear();
	Oled_Show_Str(2,2,"-----ready-----");
	
	while(1){
			if(A25 == 0 && A26 == 1 && A27 == 1){	
			/*if(mark != XJ){
				OLED_Clear();
				Oled_Show_Str(2,2,"-----XunJi----");
			}
			mark = XJ;*/
			Oled_Show_Str(2,2,"-----XunJi----");
			car_trace();
		}
		if(A25 == 1 && A26 == 0 && A27 == 1){
			/*if(mark != GS){
				OLED_Clear();
				Oled_Show_Str(2,2,"-----GenSui----");
			}
			mark = GS;*/
			Oled_Show_Str(2,2,"-----GenSui----");
			
			car_follow();
		}
		if(A25 == 1 && A26 == 1 && A27 == 0){
			/*if(mark != BZ){
				OLED_Clear();
				Oled_Show_Str(2,2,"-----BiZhang----");
			}
			mark = BZ;*/
			Oled_Show_Str(2,2,"-----BiZhang----");
			car_elude();
		}
	}
}